﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace NavigateTools
{
    class Navigation
    {
        double R = 6378137; // m

        public double Radius = 50; // m

        public double Latitude1 = 0;
        public double Longitude1 = 0;
        public double Latitude2 = 0;
        public double Longitude2 = 0;

        public double Heading = 0;
        public double Distance = 0;

        public void Calculate()
        {
            double Lat1 = Latitude1 * Math.PI / 180;
            double Lon1 = Longitude1 * Math.PI / 180;
            double Lat2 = Latitude2 * Math.PI / 180;
            double Lon2 = Longitude2 * Math.PI / 180;
            double bearing;
            double dLon = Lon2 - Lon1;
            double CosLat2 = Math.Cos(Lat2);
            double y = Math.Sin(dLon) * CosLat2;
            double x = Math.Cos(Lat1) * Math.Sin(Lat2) -
                       Math.Sin(Lat1) * CosLat2 * Math.Cos(dLon);
            bearing = Math.Atan2(y, x) / Math.PI * 180;

            x = (Lon2-Lon1) * Math.Cos((Lat1+Lat2)/2);
            y = (Lat2-Lat1);
            Distance = Math.Sqrt(x*x + y*y) * R;

            Heading = (bearing + 90 * Math.Pow(Radius / Distance, 2) + 540) % 360 - 180;
        }

    }
}
